// FlightCommand.pde is responsible for decoding transmitter stick combinations
// for setting up AeroQuad modes such as motor arming and disarming
#ifndef _AQ_FLIGHT_COMMAND_READER_
#define _AQ_FLIGHT_COMMAND_READER_
/**
 * readPilotCommands
 *
 * This function is responsible to read receiver
 * and process command from the users
 */
void readPilotCommands() {
    readReceiver();
    if (receiverCommand[THROTTLE] < MINCHECK) {
        zeroIntegralError();
        // Disarm motors (left stick lower left corner)
        if (receiverCommand[ZAXIS] < MINCHECK && motorArmed == ON) {
            commandAllMotors(MINCOMMAND);
            digitalWrite(LED_Red, LOW);
            motorArmed = OFF;
        }
        // Zero Gyro and Accel sensors (left stick lower left, right stick lower right corner)
        if ((receiverCommand[ZAXIS] < MINCHECK) && (receiverCommand[XAXIS] > MAXCHECK) && (receiverCommand[YAXIS] < MINCHECK)) {
            calibrateGyro();
            computeAccelBias();
            storeSensorsZeroToEEPROM();
            calibrateKinematics();
            zeroIntegralError();
            pulseMotors(3);
        }
        // Arm motors (left stick lower right corner)
        if (receiverCommand[ZAXIS] > MAXCHECK && motorArmed == OFF && safetyCheck == ON) {
            zeroIntegralError();
            commandAllMotors(MINTHROTTLE);
            digitalWrite(LED_Red, HIGH);
            motorArmed = ON;
        }
        // Prevents accidental arming of motor output if no transmitter command received
        if (receiverCommand[ZAXIS] > MINCHECK) {
            safetyCheck = ON;
        }
        // If motors armed, and user starts to arm/disarm motors (yaw stick hasn't passed MAXCHECK or MINCHECK yet)
        // This prevents unwanted spinup of motors
        if (motorArmed == ON) {
            commandAllMotors(MINTHROTTLE);
        }
    }
#ifdef RateModeOnly
    flightMode = RATE_FLIGHT_MODE;
#else
    // Check Mode switch for Acro or Stable
    if (receiverCommand[MODE] > 1500) {
        flightMode = ATTITUDE_FLIGHT_MODE;
    } else {
        flightMode = RATE_FLIGHT_MODE;
    }
#endif
}
#endif // _AQ_FLIGHT_COMMAND_READER_
